#include "WPILib.h"
#include "Joysticks.hpp"
#include "RobotDrive2012.hpp"
#include "math.h"
#include "Shooter.hpp"
//#include "smartGyro4.cpp"
#include "simplegyro.hpp"
#include "antislip.hpp"
#include "Scaler.hpp"
#include "Jogger.hpp"
#include "Stepper.hpp"

//#define AUTONOMOUS
#define AUTONOMOUS_SHOOT
#define AUTONOMOUS_BRIDGE
#define JAGUARS
#define BRAKE_HARD
#define TEMPSHOOTER

#define AUTO_SHOOT_SPEED -1
#define AUTO_SHOOT_TIME 3
#define AUTO_TRAVEL_SPEED .5
#define AUTO_TRAVEL_TIME 5
#define AUTO_WAIT_FOR_MOTOR 3
#define AUTO_GATE_TRAVEL_TIME 3

#define SHOOTERCHANNEL 7
#define BRIDGECHANNEL 8
#define SIDEFEEDERCHANNEL 10
#define BOTTOMFEEDERCHANNEL 11
#define GATECHANNEL 9
#define GCHANNEL 1

#define GATESTARTTIME -.2
#define GATEENDTIME .1
#define WAITTIME 0.005
#define SHOOTER_DEADBAND 0.25


#define DEAD_SPACE  0.15
#define ACCX  0.05
#define ACCY  0.05
#define BRIDGEACTIVETIME 5


#define YBALLANCEFACTOR 0.3
#define XBALLANCEFACTOR 0.3
#define GZHOLDLIMITN XBALLANCEFACTOR*0.6
#define GZHOLDLIMITP XBALLANCEFACTOR*0.85
#define GZHOLDMULTP GZHOLDLIMITP/16
#define GZHOLDMULTN GZHOLDLIMITN/16
#define DEFAULTSTEPTIME .1
#define DEFAULTROCKTIME .5

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	//RobotDrive myRobot; // robot drive system
	XBox360 stick; // only joystick
	RobotDrive2012 myRobot2012;
//	CANJaguar ShooterJag,SideFeederJag,BottomFeederJag,GateJag;
	 Gyro GZHold;
public:
	RobotDemo(void):
		//myRobot(1, 2),	// these must be initialized in the same order
		stick(1)		// as they are declared above.
		,myRobot2012()
	,GZHold(GCHANNEL)

	{
		printf("Entering Constructor\n");
		myRobot2012.SetExpiration(3);
		myRobot2012.SetSafetyEnabled(false);
		myRobot2012.SetNeutral(CANJaguar::kNeutralMode_Brake);
		printf("Robot2012\n");
	}

	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
#ifdef AUTONOMOUS
		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(0.5, 0.0); 	// drive forwards half speed
		//Wait(2.0); 				//    for 2 seconds
		//myRobot.Drive(0.0, 0.0); 	// stop robot
		
		myRobot2012.SetSafetyEnabled(false);
#ifdef AUTONOMOUS_SHOOT
		ShooterJag.Set(AUTO_SHOOT_SPEED);
		Wait(AUTO_WAIT_FOR_MOTOR);
		GateJag.Set(1.0);
		Wait(AUTO_GATE_TRAVEL_TIME);
		GateJag.Set(0);
		Wait(AUTO_SHOOT_TIME);
#endif
#ifdef AUTONOMOUS_BRIDGE
		BridgeJag.Set(1);
		myRobot2012.Drive(AUTO_TRAVEL_SPEED, 0);
		Wait(AUTO_TRAVEL_TIME);
#endif

#endif

		

	}
	void Disabled()
	{
		while (IsDisabled())
		{
			
			Wait(WAITTIME);	
			myRobot2012.ArcadeDrive(0,0);
		}
	}
	void RobotInit()
	{
		
		while (IsDisabled())
		{
			
			Wait(WAITTIME);	
			myRobot2012.ArcadeDrive(0,0);
		}
	}
	void OperatorControl(void)
	{
		myRobot2012.SetSafetyEnabled(false);
		SimpleDeadband DeadBand(0.15);
		toggle Toggle , Toggle2, Toggle3,Toggle4;

		float YDrive;			// raw value before deadband
		float XDrive;			// raw value before deadband
		scaler AccelerationCtrlX(ACCX),AccelerationCtrlY(ACCY);
		DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
		dsLCD->PrintfLine(DriverStationLCD::kUser_Line1, "Hello World no SMD");
		dsLCD->UpdateLCD();
		int iCount=200;
		bool IsBallanceMode=false;
		myRobot2012.SetNeutral(CANJaguar::kNeutralMode_Brake);
		Stepper Step(DEFAULTSTEPTIME);
		Jogger Rock(DEFAULTROCKTIME);

		while (IsOperatorControl())
		{
//			float XDrive=0,YDrive=0;
			// set the mode when a jaguar is in neutral (fed a value of zero)
			
			// Get the raw values from the joystick
			YDrive = stick.GetLeftStickY();
			XDrive = stick.GetLeftStickX();
	
		
			// Add acceleration control so the robot doesn't tip over	
			if (stick.GetButtonLeftBumper())
			{
				YDrive = YDrive*0.5;
				XDrive = XDrive*0.5;
			}					

			AccelerationCtrlX.GetSpeed(XDrive);
			AccelerationCtrlY.GetSpeed(YDrive);
			DeadBand.ModifyValue(XDrive);
			DeadBand.ModifyValue(YDrive);

				
				// BEGIN Semi auto tilt control
				if(!IsBallanceMode)IsBallanceMode=stick.GetButtonStart();
				if(IsBallanceMode)IsBallanceMode=!stick.GetButtonBack();

					if ( IsBallanceMode)
					{
						YDrive = YDrive*YBALLANCEFACTOR;
						XDrive =XDrive*XBALLANCEFACTOR;
						
						if(stick.GetButtonRightStick())
						{
							Step.Restart(stick.GetRightStickY());
							XDrive=0;
						}
						 if(stick.GetButtonLeftStick())
						{
							Rock.Restart(stick.GetRightStickY());
							XDrive=0;
						}
						 YDrive=Step.GetCurrentSpeed(YDrive);
						 YDrive=Rock.GetCurrentSpeed(YDrive);
//							printf("YDrive= %f",YDrive);
						if(YDrive==0)// Use Gyro to stabalize robot at zero
						{
							if(GZHold.GetAngle()>0.0)YDrive=GZHold.GetAngle()*GZHOLDMULTP; // Halt robot on bridge at any angle
							if(GZHold.GetAngle()<0.0)YDrive=GZHold.GetAngle()*GZHOLDMULTN; // Halt robot on bridge at any angle
							if((YDrive>0)&&(YDrive<GZHold.GetAngle()))YDrive=GZHold.GetAngle()*GZHOLDMULTP; // Halt robot on bridge at any angle
							if((YDrive<0)&&(YDrive>GZHold.GetAngle()))YDrive=GZHold.GetAngle()*GZHOLDMULTP; // Halt robot on bridge at any angle
							if(YDrive>GZHOLDLIMITP)YDrive=GZHOLDLIMITP;
							if(YDrive<-GZHOLDLIMITN)YDrive=-GZHOLDLIMITN;
							if(iCount<=0)
							{
							iCount = 200;
							printf("GZYDrive= %f, Angle=%f\n ",YDrive,GZHold.GetAngle());
							}
							iCount--;
						}
					}
					else
					{
						GZHold.Reset();
						// Place shooter code here
					}
//					printf("YDrive= %f\n",YDrive);
					if (stick.GetButtonRightBumper())
					{
						YDrive = 0;
						XDrive = 0;
					}					

						
					myRobot2012.ArcadeDrive(YDrive,XDrive);
				
				
				
				
			//**************************************************************

			// BEGIN - Feeder Control
			
			Wait(WAITTIME);				// wait for a motor update time
		}

	}

};

START_ROBOT_CLASS(RobotDemo);

